﻿/*
 * Roboshop 2.0.4
手动控制机器人行走发送速度逻辑：
1.根据最大速度计算机器人到最大速度前的增值： 增值 = 最大速度/1000.0*30 (30 为每次发送速度间隔 ms 对应Config.ini中ControlTimeOutMs值)
2.当机器人急停时所有速度置为：0
3.当机器人被阻挡时所有速度置为：增值

 Roboshop 2.0.6
手动控制机器人行走发送速度逻辑：不再判断机器人是否被阻挡拍急停，只发送最大速度，由Robokit自行控制
*/
#include "ControlTimer.h"
#include "PublicClass/PublicClass.h"
#include <QSettings>
#ifdef Q_OS_WIN
#include <Windows.h>
#include <winuser.h>
#else
#include <X11/Xlib.h>
#include <X11/Xutil.h>
//X11 输入 xev 并回车查看键码.
//A 的键码确实是 38 - 0x26
//W - 25 - 0x19
//D - 40 - 0x28
//S - 39 - 0x27
//Q - 24 - 0x18
//E - 26 - 0x1A
//R - 27 - 0x1B
#endif

namespace RO_Robot{
#ifdef Q_OS_UNIX
Display * m_display = nullptr;
#endif

ControlTimer::ControlTimer(QObject *parent) : QObject(parent)
{
    //    _timeOutMs = PublicClass::init()->readIni(PublicClass::init()->getConfigIniFilePath(),"robot/ControlTimeOutMs").toInt();
    //当按下快捷键后每200毫秒发送一次速度， robokit会在未接收到速度后自动停止 500ms, 参数配置中:ControlMotionDuration字段
    _timeOutMs = 200;

#ifdef Q_OS_WIN32
    //!!!FIX 测试反馈使用子线程会导致叉车在行走时打舵角不会返回.
    // startKeyboardThread();
    _dogTimer = new QTimer(this);
    _dogTimer->setInterval(500);//ms
    connect(_dogTimer,&QTimer::timeout,this,[=](){
        if(_controlType == E_Keyboard){
            SHORT a = GetAsyncKeyState('A');
            if(a == 0) releaseKey("A",this->metaObject()->className());

            SHORT w = GetAsyncKeyState('W');
            if(w == 0) releaseKey("W",this->metaObject()->className());

            SHORT s = GetAsyncKeyState('S');
            if(s == 0) releaseKey("S",this->metaObject()->className());

            SHORT d = GetAsyncKeyState('D');
            if(d == 0) releaseKey("D",this->metaObject()->className());

            SHORT e = GetAsyncKeyState('E');
            if(e == 0) releaseKey("E",this->metaObject()->className());

            SHORT r = GetAsyncKeyState('R');
            if(r == 0) releaseKey("R",this->metaObject()->className());
            //是否有定时器在运行.
            if(isHasTimerRunning()){
                if(!_isControlState){
                    _isControlState = true;
                    emit sigControlStateChanged(_isControlState);
                }
            }else{
                if(_isControlState){
                    _isControlState = false;
                    emit sigControlStateChanged(_isControlState);
                }
            }
        }
    });
    _dogTimer->start();
#else
    m_display = XOpenDisplay(nullptr);
    if (!m_display) {
        SCWarning<<"Cannot open display";
    }else {
        startKeyboardThread();
    }
#endif
}

ControlTimer::~ControlTimer()
{
    _isRelease = true;
    QMap<QString,QTimer*>::iterator it = _timerMap.begin();
    while(it != _timerMap.end()){
        it.value()->stop();
        it = _timerMap.erase(it);
    }
#ifdef Q_OS_UNIX
    if (m_display) {
        XCloseDisplay(m_display);
    }
#endif
}
void ControlTimer::startKeyboardThread()
{
    auto thread = new WorkerThread;
    connect(thread,&QThread::finished,thread,&QThread::deleteLater);
    thread->threadFunc(std::bind(&ControlTimer::keyboardEvent,this));
    thread->start();
}
void ControlTimer::keyboardEvent()
{

#ifdef Q_OS_WIN
        while(!_isRelease){
        if(_controlType == E_Keyboard){
            SHORT a = GetAsyncKeyState('A');
            if(a == 0) releaseKey("A",this->metaObject()->className());

            SHORT w = GetAsyncKeyState('W');
            if(w == 0) releaseKey("W",this->metaObject()->className());

            SHORT s = GetAsyncKeyState('S');
            if(s == 0) releaseKey("S",this->metaObject()->className());

            SHORT d = GetAsyncKeyState('D');
            if(d == 0) releaseKey("D",this->metaObject()->className());

            SHORT e = GetAsyncKeyState('E');
            if(e == 0) releaseKey("E",this->metaObject()->className());

            SHORT r = GetAsyncKeyState('R');
            if(r == 0) releaseKey("R",this->metaObject()->className());
            // if(a == 0
            //         && w == 0
            //         && s == 0
            //         && d == 0
            //         && e == 0
            //         && r == 0){
            //     // if(_isControlState){
            //     //     _isControlState = false;
            //     //     emit sigControlStateChanged(_isControlState);
            //     // }
            // }else{
            //是否有定时器在运行.
            if(isHasTimerRunning()){
                if(!_isControlState){
                    _isControlState = true;
                    emit sigControlStateChanged(_isControlState);
                }
            }else{
                if(_isControlState){
                    _isControlState = false;
                    emit sigControlStateChanged(_isControlState);
                }
            }
            //SCDebug<<"a:"<<a<<"w:"<<w<<"s:"<<s<<"d:"<<d<<"e:"<<e<<"r:"<<r;
        }
        QThread::msleep(500);//ms
    }
#endif
#ifdef Q_OS_UNIX
    auto  isKeyPressed= [=](char keys[],int keycode){
        int shift = keycode >> 3;
        int mask = 1 << (keycode & 7);
        if (keys[shift] & mask) {
            return true;
        }
        return false;
    };
    while(!_isRelease){
        if(m_display){
            if(_controlType == E_Keyboard){
                char keys[32] = {0};
                XQueryKeymap(m_display, keys);
                if(!isKeyPressed(keys,0x26)){
                    releaseKey("A",this->metaObject()->className());
                }
                if(!isKeyPressed(keys,0x19)){
                    releaseKey("W",this->metaObject()->className());
                }
                if(!isKeyPressed(keys,0x27)){
                    releaseKey("S",this->metaObject()->className());
                }
                if(!isKeyPressed(keys,0x28)){
                    releaseKey("D",this->metaObject()->className());
                }
                if(!isKeyPressed(keys,0x1A)){
                    releaseKey("E",this->metaObject()->className());
                }
                if(!isKeyPressed(keys,0x1B)){
                    releaseKey("R",this->metaObject()->className());
                }
                //是否有定时器在运行.
                if(isHasTimerRunning()){
                    if(!_isControlState){
                        _isControlState = true;
                        emit sigControlStateChanged(_isControlState);
                    }
                }else{
                    if(_isControlState){
                        _isControlState = false;
                        emit sigControlStateChanged(_isControlState);
                    }
                }
            }
        }
        QThread::msleep(500);//ms
    }
#endif
}
void ControlTimer::setKeyTimer(const QString &key,QPushButton *& btn)
{
    connect(btn,SIGNAL(pressed()),this,SLOT(slotButtonPrssed()));
    connect(btn,SIGNAL(released()),this,SLOT(slotButtonReleased()));

    QTimer *timer = new QTimer(this);
    connect(timer,SIGNAL(timeout()),this,SLOT(slotTimeOut()));
    timer->setProperty(M_Key,key);

    _timerMap.insert(key,timer);
    _buttonMap.insert(key,btn);
}

void ControlTimer::setKeyTimer(DirectionalPad *&directionalpad,QPainterPath *&btnArea,const QString &Key)
{
    if (!directionalpad || !btnArea) {
        qWarning() << "Invalid directionalpad or btnArea";
        return;
    }

    // 清理旧的定时器（避免重复添加）
    if (_timerMap.contains(Key)) {
        delete _timerMap.value(Key);
        _timerMap.remove(Key);
    }

    QTimer* timer = new QTimer(this);
    connect(timer, &QTimer::timeout, this, &ControlTimer::slotTimeOut);
    timer->setProperty(M_Key, Key);

    static const QHash<QString, std::pair<const char*, const char*>> keyToSignals = {
        {"W", {SIGNAL(sigDirectionUp(QString)), SIGNAL(sigReleaseDirectionUp(QString))}},
        {"S", {SIGNAL(sigDirectionDown(QString)), SIGNAL(sigReleaseDirectionDown(QString))}},
        {"A", {SIGNAL(sigDirectionLeftRotate(QString)), SIGNAL(sigReleaseDirectionLeftRotate(QString))}},
        {"D", {SIGNAL(sigDirectionRightRotate(QString)), SIGNAL(sigReleaseDirectionRightRotate(QString))}},
        {"Q", {SIGNAL(sigPause(QString)), SIGNAL(sigReleasePause(QString))}}
    };

    auto signalPair = keyToSignals.value(Key);
    connect(directionalpad, signalPair.first, this, SLOT(slotButtonAreaPressed(QString)));
    connect(directionalpad, signalPair.second, this, SLOT(slotButtonAreaReleased(QString)));
    _timerMap.insert(Key, timer);
    _buttonArea.insert(Key, btnArea);
}

void ControlTimer::slotButtonPrssed()
{
    QPushButton *btn = qobject_cast<QPushButton*>(sender());
    auto key = btn->property(M_Key).toString();
    setControlType(ControlTimer::E_Button);
        startMove(key,false);
}
void ControlTimer::slotButtonReleased()
{
    QPushButton *btn = qobject_cast<QPushButton*>(sender());
    auto key = btn->property(M_Key).toString();
    if(key != "Q"){//按钮中的Q和叫Q的定时器不同
        releaseKey(key);
    }
}

void ControlTimer::slotButtonAreaPressed(QString key)
{
    if(key == "Q"){
        emergencyStop();
    }else{
        setControlType(ControlTimer::E_Button);
        startMove(key,false);
    }
}

void ControlTimer::slotButtonAreaReleased(QString key)
{
    if(key != "Q"){
        releaseKey(key);
    }
}

//------
void ControlTimer::slotControlParameter(qreal dMaxSpeed,
                                        qreal dMaxSpeedAcc,
                                        qreal dMaxRot,
                                        qreal dMaxRotAcc)
{
    QSettings write(PublicClass::init()->getTaskConfigIniFilePath(), QSettings::IniFormat);//当前目录的INI文件.
    QString key = "speedModel";
    if(4 == _chassisMode){
        key = "speedModel4";
    }
    write.setValue(key+"/maxRot",dMaxRot);
    //    write->setValue(key+"/maxRotAcc",dMaxRotAcc);
    write.setValue(key+"/maxSpeed",dMaxSpeed);
    //    write->setValue(key+"/maxSpeedAcc",dMaxSpeedAcc);

    setMaxSpeed(dMaxSpeed);
    setMaxRot(dMaxRot);
}

int ControlTimer::getSteerValueAngleH() const
{
    return _steerValueAngleH;
}

void ControlTimer::setSteerValueAngleH(int steerValueAngleH)
{
    _steerValueAngleH = steerValueAngleH;
}

int ControlTimer::chassisMode() const
{
    return _chassisMode;
}

void ControlTimer::setChassisMode(int mode)
{
    _chassisMode = mode;
}

QMap<QString, QPushButton *> ControlTimer::buttonMap() const
{
    return _buttonMap;
}

QMap<QString,QPainterPath*> ControlTimer::buttonArea() const
{
    return _buttonArea;
}

void ControlTimer::setButtonMap(const QMap<QString, QPainterPath *> &value)
{
    _buttonArea = value;
}
void ControlTimer::setButtonMap(const QMap<QString, QPushButton *> &value)
{
    _buttonMap = value;
}

void ControlTimer::slotAutoStopTimeOut()
{
    if(!_timerMap["Q"]->isActive()){
        stopAllTimer();
        _timerMap["Q"]->start(500/*_timeOutMs*/);
    }
}

void ControlTimer::setButtonChecked(const QString &key,bool checked)
{
    if(_timerMap.contains(key)&&_buttonMap.contains(key)){
        _buttonMap[key]->setChecked(checked);
    }
}

bool ControlTimer::isHasTimerRunning()
{
    QMap<QString,QTimer*>::iterator it = _timerMap.begin();
    while(it != _timerMap.end()){
        if(it.value()->isActive()){
            return true;
        }
        ++it;
    }
    return false;
}
//衰减速度停止
void ControlTimer::attenuationSpeedStop()
{
    SCDebug<<QStringLiteral("衰减速度直到完全停止");
    if(isHasTimerRunning()){
        QMap<QString,QTimer*>::iterator it = _timerMap.begin();
        while(it != _timerMap.end()){
            if(it.key() != "Q"){
                it.value()->stop();
                if(_buttonMap.contains(it.key()))
                {
                    _buttonMap[it.key()]->setChecked(false);
                }
                if(_buttonArea.contains(it.key()))
                {
                    emit sigKeyUnChecked(it.key());
                }
            }
            ++it;
        }
        if(!_timerMap["Q"]->isActive()){
            _timerMap["Q"]->start(5);
        }
    }
}

void ControlTimer::releaseKey(const QString &key,const QString &fromClassName)
{
    if(_chassisMode == 4 && isCompatibilityMode() && key == "E"){//E是舵轮归零快捷键
        return;
    }else if(key == "AQ"){
        //        emergencyStop();//底层控制速度，上层不再控制衰减
        attenuationSpeedStop();
    }else if(_timerMap.contains(key)){

        if(_timerMap[key]->isActive()){
            SCWarning<<"fromClassName:"<<fromClassName<<"releaseKey:"<<key;
            _timerMap[key]->stop();
            //_timerMap[key]->setProperty(M_StartMoveTimeMS,QVariant());
            if(_buttonMap.contains(key))
            {
                _buttonMap[key]->blockSignals(true);
                _buttonMap[key]->setChecked(false);
                _buttonMap[key]->blockSignals(false);
            }
            if(_buttonArea.contains(key))
            {
                emit sigKeyUnChecked(key);
            }
            if(key !="Q" && !_timerMap["Q"]->isActive()){

                if(_chassisMode == 4 && isCompatibilityMode()  && (key=="A" || key=="D")){

                    SCDebug<<QStringLiteral("舵轮A,D松开不开启停止定时器.立马发 0 ");
                    _steerValueAngleH = 0;
                    if(isHasTimerRunning()){
                        emit sigControlSpeed(2,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
                    }else{
                        emit sigControlSpeed(3,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
                    }
                }else{
                    _timerMap["Q"]->start(5);
                }
            }
        }
    }else{
        SCDebug<<"ControlTimer::releaseKey:_timerMap.keys(): not the "<<key;
    }
}
void ControlTimer::releaseKey(const QKeySequence &keyS)
{
    QString key = keyS.toString(QKeySequence::NativeText);
    releaseKey(key,this->metaObject()->className());
}

//开始移动,是否开启自动停止定时器
void ControlTimer::startMove(const QString &key,bool autoStop)
{
    if(_chassisMode == 4 && isCompatibilityMode() && key == "E"){
        slotZeroAngle();
    }else{
        if(_timerMap.contains(key)){
            if(!_timerMap[key]->isActive()){
                if(_buttonMap.contains(key)){
                    _buttonMap[key]->setChecked(true);
                }
                if(_buttonArea.contains(key))
                {
                    emit sigKeyChecked(key);
                }
                _timerMap[key]->start(_timeOutMs);//设置时间
            }
        }else{
            SCDebug<<"ControlTimer::startMove: _timerMap.keys(): not the"<<key;
        }
    }
}

/** 外部快捷键控制（Xbox使用）
 * @brief ControlTimer::slotShortcut
 */
void ControlTimer::slotShortcut()
{
    QShortcut *cut = qobject_cast<QShortcut*>(sender());
    QString key = cut->key().toString(QKeySequence::NativeText);
    startMove(key,true);
}

qreal ControlTimer::maxRot() const
{
    return _maxRot;
}

void ControlTimer::setMaxRot(const qreal &maxRot)
{
    _maxRot = maxRot;
}

qreal ControlTimer::maxSpeed() const
{
    return _maxSpeed;
}

void ControlTimer::setMaxSpeed(const qreal &maxSpeed)
{
    _maxSpeed = maxSpeed;
}

void ControlTimer::slotTimeOut()
{
    QTimer *timer = qobject_cast<QTimer*>(sender());
    if(!timer){
        SCDebug<<"timer is null";
    }else{
        //如果是舵轮，且为兼容模式
        if(4 == _chassisMode && isCompatibilityMode()){//舵轮左
            steerModeFuntion(timer);
        }else{
            modeFuntion(timer);
        }
    }
}

/**清除所有按钮状态
 * @brief ControlTimer::clearAllBtnState
 */
void ControlTimer::clearAllBtnState()
{
    QMap<QString,QPushButton*>::iterator it = _buttonMap.begin();
    while(it != _buttonMap.end()){
        it.value()->setChecked(false);
        ++it;
    }
}

void ControlTimer::clearAllBtnAreaState()
{
    QMap<QString,QPainterPath*>::iterator it = _buttonArea.begin();
    while(it != _buttonArea.end()){
        emit sigKeyUnChecked(it.key());
        ++it;
    }
}

//停止所有定时器
void ControlTimer::stopAllTimer()
{
    QMap<QString,QTimer*>::iterator it = _timerMap.begin();
    while(it != _timerMap.end()){
        it.value()->stop();
        ++it;
    }
}

/**紧急停止
 * @brief ControlTimer::emergencyStop
 */
void ControlTimer::emergencyStop()
{
    SCDebug<<QStringLiteral("紧急停止Q");
    stopAllTimer();
    stopMoving(3);
}

/**停止发送速度
 * @brief ControlTimer::stopMoving
 * @param type 0差动，全动，1差动、全动停止， 2舵轮 ，3舵轮停止(左转-1，右转1，停止0)
 */
void ControlTimer::stopMoving(int type)
{
    _vxSpeed = 0;
    _vySpeed = 0;
    _wSpeed = 0;
    _steerValueAngleH = 0;
    clearAllBtnState();
    clearAllBtnAreaState();
    SCDebug<<"stopMoving: "<<"type:"<<type<<"vx:"<<_vxSpeed<<"vy:"<<_vySpeed
            <<"w:"<<_wSpeed<<"steer:"<<_steerValueAngleH;
    emit sigControlSpeed(type,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
}

//移动发送速度
//type = 0差动，全动，1差动、全动停止， 2舵轮 ，3舵轮停止
void ControlTimer::setMoving(int type,qreal vx,qreal vy,qreal w,qreal steer)
{
    SCDebug<<"setMoving:"<<"type:"<<type<<"vx:"<<vx<<"vy:"<<vy<<"w:"<<w<<"steer:"<<steer;
    if(4 == chassisMode()){
        type = 2;
    }
    emit sigControlSpeed(type,vx,vy,w,steer);
}
//mode = 1,2,3函数
void ControlTimer::modeFuntion(QTimer*timer)
{
    auto key = timer->property(M_Key).toString();
    if(key.isEmpty()) return;
    if(key == "W"){
        _controlSendMutex.lock();
        _vxSpeed =  _maxSpeed;
        setMoving(0,_vxSpeed,_vySpeed,_wSpeed,0);
        _controlSendMutex.unlock();

    }else if(key == "S"){
        _controlSendMutex.lock();
        _vxSpeed = -_maxSpeed;
        setMoving(0,_vxSpeed,_vySpeed,_wSpeed,0);
        _controlSendMutex.unlock();

    }else if(key == "A"){
        _controlSendMutex.lock();

        _wSpeed = qDegreesToRadians(_maxRot) ;

        setMoving(0,_vxSpeed,_vySpeed,_wSpeed,0);
        qDebug()<<"_vxSpeed is"<<_vxSpeed<<"_wSpeed is"<<_wSpeed;
        _controlSendMutex.unlock();

    }else if(key == "D"){
        _controlSendMutex.lock();

        _wSpeed = (-1)*qDegreesToRadians(_maxRot) ;

        setMoving(0,_vxSpeed,_vySpeed,_wSpeed,0);
        qDebug()<<"_vxSpeed is"<<_vxSpeed<<"_wSpeed is"<<_wSpeed;
        _controlSendMutex.unlock();

    }else if(key == "E"){
        _controlSendMutex.lock();

        _vySpeed =  _maxSpeed;

        setMoving(0,_vxSpeed,_vySpeed,_wSpeed,0);
        _controlSendMutex.unlock();

    }else if(key == "R"){
        _controlSendMutex.lock();
        _vySpeed = (-1)* _maxSpeed;
        setMoving(0,_vxSpeed,_vySpeed,_wSpeed,0);
        _controlSendMutex.unlock();

    }else if(key == "Q"){
        //利用_stopTimer2定时器来递减停止机器人，根据松开的键，进行速度递减
        //考虑到会同时按住wa,wd,sd,sa等情况，所有区分停止
        _controlSendMutex.lock();

        if(!_timerMap["W"]->isActive() && !_timerMap["S"]->isActive()){
            _vxSpeed = 0;
        }
        if(!_timerMap["E"]->isActive() && !_timerMap["R"]->isActive()){
            _vySpeed = 0;
        }

        if(!_timerMap["A"]->isActive() && !_timerMap["D"]->isActive()){
            _wSpeed = 0;
        }
        _timerMap["Q"]->stop();
        emit sigControlSpeed(0,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
        _controlSendMutex.unlock();
    }
}
//mode = 4 舵轮
void ControlTimer::steerModeFuntion(QTimer *timer)
{
    auto key = timer->property(M_Key).toString();
    if(key == "W"){
        _controlSendMutex.lock();
        _vxSpeed =  _maxSpeed;
        setMoving(2,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
        _controlSendMutex.unlock();

    }else if(key == "S"){
        _controlSendMutex.lock();
        _vxSpeed = (-1)* _maxSpeed;
        setMoving(2,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
        _controlSendMutex.unlock();

    }else if(key == "A"){
        _controlSendMutex.lock();
        _steerValueAngleH = 1;
        setMoving(2,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
        _controlSendMutex.unlock();

    }else if(key == "D"){
        _controlSendMutex.lock();
        _steerValueAngleH = -1;
        setMoving(2,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
        SCDebug<<"_vxSpeed is"<<_vxSpeed<<"_wSpeed is"<<_wSpeed;
        _controlSendMutex.unlock();

    }else if(key == "Q"){
        //利用_stopTimer2定时器来递减停止机器人，根据松开的键，进行速度递减
        //考虑到会同时按住wa,wd,sd,sa等情况，所以区分停止
        _controlSendMutex.lock();

        if(!_timerMap["W"]->isActive() && !_timerMap["S"]->isActive()){
            _vxSpeed = 0;
        }
        if(!_timerMap["A"]->isActive() && !_timerMap["D"]->isActive()){
            _wSpeed = 0;
        }
        _timerMap["Q"]->stop();
        emit sigControlSpeed(2,_vxSpeed,_vySpeed,_wSpeed,_steerValueAngleH);
        _controlSendMutex.unlock();
    }
}
//舵轮归零
void ControlTimer::slotZeroAngle()
{
    setMoving(2,_vxSpeed,_vySpeed,_wSpeed,-2);
}


ControlTimer::ControlType ControlTimer::controlType() const
{
    return _controlType;
}

void ControlTimer::setControlType(ControlTimer::ControlType newControlType)
{
    _controlType = newControlType;
}

bool ControlTimer::isCompatibilityMode() const
{
    return _isCompatibilityMode;
}

void ControlTimer::setCompatibilityMode(bool isCompatibilityMode)
{
    _isCompatibilityMode = isCompatibilityMode;
}
}//namespace
